function cen = rs_correction(omega, cen_in, pars, Trd)
% rolling shutter correction function
% omega in nx3 or 3xn matrix form

if nargin == 3
    Trd = pars(10);  
    Vs = pars(11);
elseif nargin == 4
    Vs = 1/Trd;
else
    error('invalid number of input arguments');
end

    CenterX = pars(1);
    CenterY = pars(2);
    dx = pars(3);
    f = pars(5);
    pix_unit = f/dx;
    
    % omega format: 3xn
    if size(omega,1) ~= 3
        error('invalid omega input!');
    else
        wx = omega(1,:);
        wy = omega(2,:);
        wz = omega(3,:);
    end
    
    % for example, 13 star,x and y 13x1, coordinates 13x2 
    x = cen_in(:,1) - CenterX;
    y = cen_in(:,2) - CenterY;              
    
    % 10 omg cand, make Vx and Vy 13x10
    Vx = wz.*y + wy.*pix_unit + wy.*x.^2/pix_unit - wx.*x.*y/pix_unit;      
    Vy = -wz.*x - wx.*pix_unit - wx.*y.^2/pix_unit + wy.*x.*y/pix_unit;
    
    cen_y_s1 = y.*(Vs-Vy)*Trd;              %  y*Vy/(Vs-Vy)
    cen_x_s1 = x - Vx.*ceil(y)*Trd;         %  mean([cen_y_s1 y]
    
    % final result: 13x20
    cen = [cen_x_s1 + CenterX, cen_y_s1 + CenterY];         
end
